#ifndef FUSION_H
#define FUSION_H
#include <ros/ros.h>

#include <pcl/io/pcd_io.h>

#include <pcl/common/common.h>
#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/passthrough.h>



#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>

#include "darknet.h"
#include "improcess.h"
#include "matching2D.hpp"
#include "camFusion.hpp"
#include "lidarData.hpp"
#include "dataStructures.h"
#include "objectDetection2D.hpp"

#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <fstream>
#include <sstream>
#include <iostream>

#include <vector>
#include <algorithm>
#include <jsk_recognition_msgs/BoundingBox.h>
#include <jsk_recognition_msgs/BoundingBoxArray.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <eigen3/Eigen/src/Geometry/Quaternion.h>

#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/PointCloud2.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <image_transport/image_transport.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include "lidarmarkers_msg/lidarMarkers.h"
#include <deque>



class LidarCamFusion
{
public:
    LidarCamFusion(ros::NodeHandle &nh);
    ~LidarCamFusion();


    void fusionCallback(const sensor_msgs::ImageConstPtr& image, const sensor_msgs::PointCloud2ConstPtr& cloud);
    void detectObjectsCV(cv::Mat& img, std::vector<BoundingBox>& bBoxes, float confThreshold, float nmsThreshold, 
                std::string classesFile, std::string modelConfiguration, std::string modelWeights, bool bVis);
    float get_color(int c, int x, int max);
    // void lidarCluster(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);

    void yolov3Detec(cv::Mat& visImg);

    vector<Point2d> pointcloud2_to_image(const vector<Point3f> msg,
                                         const cv::Size& imageSize);


private:
    message_filters::Subscriber<sensor_msgs::Image> cam_sub_;
    message_filters::Subscriber<sensor_msgs::PointCloud2> lidar_sub_;
    message_filters::Subscriber<lidarmarkers_msg::lidarMarkers> marker_sub_;
    image_transport::Publisher pub_imgdetec;
    ros::Publisher pub_finalMarker;

    std::string cam_topic_ ;
    std::string lidar_topic_ ;
    std::string marker_topic_;

    cv::Mat P_rect_00; // 3x4 projection matrix after rectification
    cv::Mat R_rect_00; // 3x3 rectifying rotation to make image planes co-planar
    cv::Mat RT; // rotation matrix and translation vector
    cv::Mat PRT;
    
    string yoloClassesFile = "/home/zh/darknet/data/coco.names";
    string yoloModelConfiguration = "/home/zh/SFND_3D_Object_Tracking/dat/yolo/yolov3.cfg";
    string yoloModelWeights = "/home/zh/SFND_3D_Object_Tracking/dat/yolo/yolov3.weights";
    
    
    network *net;
    float thresh = 0.5;
    int classes = 80;
    
    vector<Rect> boxes;
    vector<cv::Point2d> box_center;
    vector<cv::Point2d> marker_center;
    vector<int>classNames;
    bool boxes_updated = false;
    vector<string> classNamesVec;
    float colors[6][3] = {{1,0,1},{0,0,1},{0,1,1},{0,1,0},{1,1,0},{1,0,0}};
    vector<DataFrame> dataBuffer;
};


#endif //FUSION_H